/*
 * Particle.h
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#ifndef PARTICLE_H_
#define PARTICLE_H_
#include <cmath>
#include <cstdlib>
#include <vector>
#include "Maze.h"
#include "Point.h"
#include "MathUtil.h"
#define PARTICLE_NBR 2000

namespace std {

class Particle {
public:
	double dx;
	double dy;
	double x;
	double y;
	Point p;
	double heading;
	double w;
	double radians(double d);
	void move_by(double x, double y);
	bool advance_by(double speed, Maze * m);
	bool advance_by(double speed, Maze * m, bool noisy);
	bool advance_by(double speed, Maze* m, bool noisy,bool no_checker);
	double read_sensor(Maze * maze);
	static void create_random(vector<Particle>*p,int count, Maze* maze);
	Particle():dx(0),dy(0),x(0),y(0),p(),heading(0.0),w(0){}
	Particle(Point p):dx(0),dy(0),x(p.x),y(p.y),p(p),heading(0.0),w(0){}
	Particle(double x, double y, double heading,double w);
	virtual ~Particle();

	Particle(const Particle& pa){
		dx = pa.dx;
		dy = pa.dy;
		x = pa.x;
		y = pa.y;
		p = pa.p;
		heading = pa.heading;
		w = pa.w;
	}

	Particle& operator=(const Particle &pa){
		dx = pa.dx;
		dy = pa.dy;
		x = pa.x;
		y = pa.y;
		p = pa.p;
		heading = pa.heading;
		w = pa.w;
		return *this;
	}

	static Point compute_mean_point(vector<Particle> *parts, Maze *maze){
		bool noisy = false;
		double m_x = 0.0;
		double m_y = 0.0;
		double m_count = 0.0;
		for(unsigned int i = 0; i < parts->size(); i++){
			m_count += (*parts)[i].w;
			m_x += (*parts)[i].x * (*parts)[i].x;
			m_y += (*parts)[i].y * (*parts)[i].y;
		}
		if(m_count == 0) return Point(-1,-1);
		m_x /= m_count;
		m_y /= m_count;
		m_count = 0;
		Point m(m_x,m_y);
		for (unsigned int i = 0; i < parts->size(); i++) {
			if (Maze::distance((*parts)[i].p,m) < 1.0){
				m_count ++;
			}
		}
		noisy = m_count > (PARTICLE_NBR * 0.95);
		return Point(m_x,m_y);
	}

	static double add_noise(double border, double val){
		return val+(((UNIFORM_RAND)*2*border)-border);
	}

	static double add_little_noise(double val){
		return add_noise(0.02,val);
	}

	static double add_some_noise(double val){
		return add_noise(0.1,val);
	}

	static double sigma2;
	static double w_gauss(double a, double b){
		double error = a - b;
		return pow(M_PI,-(error*error/(2*sigma2)));
	}

};

} /* namespace std */
#endif /* PARTICLE_H_ */
